#include <math.h>
#include <omp.h>

#define PI acos(-1.0)

class Not_gate
{
public:Not_gate()
{
    output = 0;
}
      void ExecuteOperation(int input)
      {
          output = 1 - abs(input) % 2;
      }
public:
    int output;
};

class Two_input_and_gate
{
public:Two_input_and_gate()
{
    output = 0;
}
      void ExecuteOperation(int input1, int input2)
      {

          int A[2], B[2];
          int i;
          int out;

          A[0] = abs(input1);
          A[1] = abs(input2);

          for (i = 0; i < 2; i++)
          {
              B[i] = A[i] % 2;
              A[i] = B[i];
          }
          out = 1;
          for (i = 0; i < 2; i++)
          {
              out = out & A[i];
          }
          output = out;
      }
public:
    int output;
};

class SR_Flip_Flop
{
public:
    SR_Flip_Flop()
    {
        output_Q = 0;
        output_Q_not = 0;
        output_Q_his = 0;
        output_Q_not_his = 0;
        input_s_his = 0;
        input_r_his = 0;
    }
    void ExecuteOperation(int input_s, int input_r)
    {
        if ((input_s > input_s_his) || (input_r > input_r_his))
        {
            if (input_r == 1)
            {
                if (input_s == 0)
                {
                    output_Q_not = 0;
                    output_Q = 1;
                }
                else
                {
                    output_Q_not = 1;
                    output_Q = 0;
                }
            }
            else
            {
                if (input_s == 0)
                {
                    output_Q_not = 0;
                    output_Q = 1;
                }
                else
                {
                    output_Q_not = 1;
                    output_Q = 0;
                }
            }
        }
        else
        {
            output_Q = output_Q_his;
            output_Q_not = output_Q_not_his;
        }
        if ((input_s < input_s_his) || (input_r < input_r_his))
        {
            if (input_r == 1)
            {
                if (input_s == 0)
                {
                    output_Q_not = 0;
                    output_Q = 1;
                }
                else
                {
                    output_Q_not = 1;
                    output_Q = 0;
                }
            }
            else
            {
                if (input_s == 0)
                {
                    output_Q_not = 0;
                    output_Q = 1;
                }
                else
                {
                    output_Q_not = 1;
                    output_Q = 0;
                }
            }
        }
        else
        {
            output_Q = output_Q_his;
            output_Q_not = output_Q_not_his;
        }

        input_r_his = input_r;
        input_s_his = input_s;
        output_Q_his = output_Q;
        output_Q_not_his = output_Q_not;
    }
public:
    int output_Q;
    int output_Q_not;
    int output_Q_his;
    int output_Q_not_his;
    int input_s_his;
    int input_r_his;
};

class LPF_SELF
{
public:
    LPF_SELF(double T_start, double DELT)
    {
        Kf = 2.0 * T_start / DELT;
        output = 0.0;
        output_his = 0.0;
        input_his = 0.0;
    }
    void ExecuteOperation(double G_start, double input)
    {
        output = 1.0 / (1 + Kf) * G_start * (input_his + input) + (Kf - 1) / (Kf + 1) * output_his;
        output_his = output;
        input_his = input;
    }
public:
    double input_his;
    double output_his;
    double Kf;
    double output;
};

class LPF
{
public:
    LPF()
    {
        output = 0.0;
        output_his = 0.0;
        input_his = 0.0;
    }

    void ExecuteOperation(double Kf, double G, double input)
    {
        output = 1.0 / (1 + Kf) * G * (input_his + input) + (Kf - 1) / (Kf + 1) * output_his;
        output_his = output;
        input_his = input;
    }

public:
    // 输出量
    double output;
    double output_his, input_his;
};

class comparator
{
public:
    comparator()
    {
        output = 0;
    }
    void ExecuteOperetion(double input_A, double input_B, double comparator_H, double comparator_L)
    {
        if (input_A > input_B)
        {
            output = comparator_H;
        }
        else
        {
            output = comparator_L;
        }
    }
public:
    double output;
};

class Divider
{
public:
    Divider()
    {
        output = 0.0;
    }
    void ExecuteOperation(double input1, double input2)
    {
        output = input1 / input2;
    }
public:
    double output;
};

class ABC_DQ0
{
public:
    ABC_DQ0()
    {
        dq00 = 0.0;
        dq01 = 0.0;
    }
    void ExecuteOperation(double abc0, double abc1, double abc2, double theta)
    {
        dq00 = (2.0 / 3.0) * (cos(theta) * abc0 + cos(theta - 2.0 * acos(-1.0) / 3.0) * abc1 + cos(theta + 2.0 * acos(-1.0) / 3.0) * abc2);
        dq01 = (2.0 / 3.0) * (sin(theta) * abc0 + sin(theta - 2.0 * acos(-1.0) / 3.0) * abc1 + sin(theta + 2.0 * acos(-1.0) / 3.0) * abc2);
        output0 = dq00;
        output1 = dq01;
    }

public:
    double dq00;
    double dq01;
    // 输出量
    double output0;
    double output1;
};
class PLL3Phase
{
public:
    PLL3Phase(double KP, double KI, double BASE, double BASEFREQ, double fmax, double fmin, double deltaT)
    {

        PIPG = KP;
        PIIT = 1.0 / KI;
        //PIIT = KI;
        Base = BASE;
        Frat = BASEFREQ;
        dt = deltaT;
        fc = 70.5;
        Fmax = fmax * Frat;
        Fmin = fmin * Frat;

        PHI_his = 0.0;
        Fmax1 = Fmax * 2.0 * PI;
        Fmin1 = Fmin * 2.0 * PI;
        input_integral_old = 0.0;
        output_integral_old = 0.0;
        Err_input_old = 0;
        Err_integral_old = 0;
        Tf = 1.0 / (2.0 * PI * fc);
        Kf = 2 * Tf / dt;
        input_his = 0;
        output_his = 0;

        vd1_1_his = 0;
        vq1_1_his = 0;
        vd2_1_his = 0;
        vq2_1_his = 0;

        output_y = 0.0;
        output_integral = 0.0;
        PI_out = 0.0;
        W1 = 0.0;
        Err_integral = 0.0;
        sumV = 0.0;
        a = 0.0;
        b = 0.0;
        vd1 = 0.0;
        vq1 = 0.0;
        vd2 = 0.0;
        vq2 = 0.0;
        vd1_1 = 0.0;
        vq1_1 = 0.0;
        vd2_1 = 0.0;
        vq2_1 = 0.0;
        vd1_L = 0.0;
        vq1_L = 0.0;
        vd2_L = 0.0;
        vq2_L = 0.0;
        output = 0.0;
    }

    double Clarka_Transformation(double input1, double input2, double input3)
    {
        double a;

        a = 2.0 / 3 * (input1 - input2 / 2 - input3 / 2) / Base;

        return a;
    }

    double Clarkb_Transformation(double input1, double input2, double input3)
    {
        double b;

        b = sqrt(3.0) / 3 * (input2 - input3) / Base;

        return b;
    }

    double Parkd_Tansformation(double a, double b, double PHI_his)
    {
        double vd1;

        vd1 = a * cos(PHI_his) + b * sin(PHI_his);
        return vd1;
    }

    double Parkq_Tansformation(double a, double b, double PHI_his)
    {
        double vq1;

        vq1 = -a * sin(PHI_his) + b * cos(PHI_his);
        return vq1;
    }

    double iParkd_Tansformation(double a, double b, double PHI_his)
    {
        double vd2;

        vd2 = a * cos(PHI_his) - b * sin(PHI_his);
        return vd2;
    }

    double iParkq_Tansformation(double a, double b, double PHI_his)
    {
        double vq2;

        vq2 = a * sin(PHI_his) + b * cos(PHI_his);
        return vq2;
    }

    double Max_Min_Limit(double Max, double Min, double PI_out)
    {

        double W1;

        if (PI_out < Max)
        {
            if (PI_out > Min)
            {
                W1 = PI_out;
            }
            else
            {
                W1 = Min;
            }
        }
        else
        {
            W1 = Max;
        }

        return W1;
    }

    double Two_PI_Limit(double W)
    {
        double PHI1;
        double W1 = W;
        if (W1 < 2.0 * PI)
        {
            if (W1 > 0)
            {
                PHI1 = W1;
            }
            else
            {
                while (W1 < 0)
                {
                    W1 = W1 + 2.0 * PI;
                }
                PHI1 = W1;
            }
        }
        else
        {
            while (W1 > 2.0 * PI)
            {
                W1 = W1 - 2.0 * PI;
            }
            PHI1 = W1;
        }

        return PHI1;
    }

    void ExecuteOperation(double inputA, double inputB, double inputC)
    {
        // Clark_transformation//
        a = Clarka_Transformation(inputA, inputB, inputC);
        b = Clarkb_Transformation(inputA, inputB, inputC);

        // Park_transformation//
        vd1 = Parkd_Tansformation(a, b, PHI_his);
        vq1 = Parkq_Tansformation(a, b, PHI_his);

        // iPark_transformation//
        vd2 = iParkd_Tansformation(a, b, PHI_his);
        vq2 = iParkq_Tansformation(a, b, PHI_his);

        sumV = inputA + inputB + inputC;

        if (sumV <= 1e-8 && sumV >= -1e-8) // three-phase equilibrium//
        {
            // PI_Backward_Euler//
            Err_integral = (dt / PIIT * 0.5) * (vq1 + Err_input_old) + Err_integral_old;
            Err_input_old = vq1;
            Err_integral_old = Err_integral;

            PI_out = PIPG * vq1 + Err_integral + 2.0 * PI * Frat;
        }
        else // three-phase imbalance//
        {

            LPF1.ExecuteOperation(Kf, 1.0, vd1_1_his);
            LPF2.ExecuteOperation(Kf, 1.0, vq1_1_his);
            LPF3.ExecuteOperation(Kf, 1.0, vd2_1_his);
            LPF4.ExecuteOperation(Kf, 1.0, vq2_1_his);

            vd1_L = LPF1.output;
            vq1_L = LPF2.output;
            vd2_L = LPF3.output;
            vq2_L = LPF4.output;

            vd1_1 = vd1 - vd2_L * cos(2 * PHI_his) - vq2_L * sin(2 * PHI_his);
            vq1_1 = vq1 + vd2_L * sin(2 * PHI_his) - vq2_L * cos(2 * PHI_his);
            vd2_1 = vd2 - vd1_L * cos(2 * PHI_his) + vq1_L * sin(2 * PHI_his);
            vq2_1 = vq2 - vd1_L * sin(2 * PHI_his) - vq1_L * cos(2 * PHI_his);

            vd1_1_his = vd1_1;
            vq1_1_his = vq1_1;
            vd2_1_his = vd2_1;
            vq2_1_his = vq2_1;

            // PI_Backward_Euler//
            Err_integral = (dt / PIIT * 0.5) * (vq1_1 + Err_input_old) + Err_integral_old;
            Err_input_old = vq1_1;
            Err_integral_old = Err_integral;

            PI_out = PIPG * vq1_1 + Err_integral + 2.0 * PI * Frat;
        }

        // max_min_limiter//
        W1 = Max_Min_Limit(Fmax1, Fmin1, PI_out);

        // integrator//
        output_integral = (input_integral_old + W1) * (dt / 2.0) + output_integral_old;
        input_integral_old = W1;
        output_integral_old = output_integral;

        output_integral = output_integral - 2.0 * PI;

        // 0_2PI_Limiter//
        output_y = Two_PI_Limit(output_integral);

        output = output_y;

        PHI_his = output;
    }

public:
    // 参数
    double dt;
    double Base;
    double PIPG;
    double PIIT;
    double Frat;
    double Fmax;
    double Fmin;
    double fc;

    // 变量
    double PHI_his, output_y;
    double output_integral;
    double input_integral_old, output_integral_old;
    double Err_input_old, Err_integral_old;
    double Fmax1, Fmin1;

    double PI_out, W1;
    double Err_integral;
    double sumV, input_his, output_his, Tf, Kf;
    double a, b;
    double vd1, vq1, vd2, vq2;
    double vd1_1, vq1_1, vd2_1, vq2_1;
    double vd1_1_his, vq1_1_his, vd2_1_his, vq2_1_his;
    double vd1_L, vq1_L, vd2_L, vq2_L;

    LPF LPF1;
    LPF LPF2;
    LPF LPF3;
    LPF LPF4;

    // 输出量
    double output;
};
class angle_resolver
{
public:
    angle_resolver()
    {
        output = 0.0;
    }
    void ExecuteOperation(double input)
    {
        double n;
        if (input > 2.0 * PI)
        {
            n = int(input / (2.0 * PI));
            output = input - 2.0 * PI * n;
        }
        else if (input < 0)
        {
            n = int(fabs(input) / (2.0 * PI));
            output = input + (n + 1) * 2.0 * PI;
        }
        else
        {
            output = input;
        }
    }
public:
    double output;
};

class signal_switch
{
public:
    signal_switch()//预处理
    {
        output = 0.0;
    }
    void ExecuteOperation(double input1, double input2, int ctrl, int signal)//具体运算
        //double ExecuteOperation(double input1, double input2, int ctrl, int signal)//具体运算
    {

        if (signal == ctrl)
            output = input1;
        // output = 0.0;
        else
            output = input2;
        //output = 0.0;
   // return output;
    }

public:
    //int signal,ctrl;
    double output;
};

class PI_model
{
public:
    PI_model()//预处理
    {
        Y1_his = 0.0;
        Y4_his = 0.0;
        Y5_his = 0.0;
        Y6_his = 0.0;
        Y7_his = 0.0;
        Y8_his = 0.0;
        Y9_his = 0.0;
        PI_OUT = 0.0;
    }
    void ExecuteOperation(double PI_IN, int RST, int FRZ, double MAXOUT, double MINOUT, double T, double Kp, double DELT)//具体运算
    {
        T1 = DELT / (2 * T);


        //Not Gate
        if (RST == 1)
            k1 = 0;
        else
            k1 = 1;

        //And Gate
        if (FRZ == 1 || Y5_his == 1 || Y4_his == 1)
            k4 = 1;
        else
            k4 = 0;

        //Comparator
        if (Y9_his > MAXOUT)
            k3 = 1;
        else
            k3 = 0;
        if (MINOUT > Y9_his)
            k2 = 1;
        else
            k2 = 0;

        //signal switch
        if (Y6_his == 1)
            k5 = 0;
        else
            k5 = 1;

        //Limiter
        if (Y9_his > MAXOUT)
        {
            k6 = 1;
            k7 = k8 = 0;
        }
        else if (Y9_his < MINOUT)
        {
            k6 = k8 = 0;
            k7 = 1;
        }
        else
        {
            k8 = 1;
            k6 = k7 = 0;
        }

        //Integretor Limiter
        if (Y8_his > MAXOUT)
        {
            k9 = 1;
            k10 = k11 = 0;
        }

        else if (Y8_his < MINOUT)
        {
            k10 = 1;
            k9 = k11 = 0;
        }
        else
        {
            k11 = 1;
            k9 = k10 = 0;
        }
        Y1 = k1;
        Y2 = PI_IN * Y1;
        Y3 = Kp * Y2;
        Y4 = k2;
        Y5 = k3;
        Y6 = k4;
        Y7 = k5 * PI_IN;
        Y8 = k9 * MAXOUT + k10 * MINOUT + k11 * (Y8_his + T1 * (Y7 + Y7_his));

        Y9 = Y3 + Y8;
        Y10 = k8 * Kp * k1 * PI_IN + k8 * Y8 + k6 * MAXOUT + k7 * MINOUT;

        //output
        PI_OUT = Y10;

        Y1_his = Y1;
        Y4_his = Y4;
        Y5_his = Y5;
        Y6_his = Y6;

        Y7_his = Y7;
        Y8_his = Y8;
        Y9_his = Y9;
        //return PI_OUT;
    }


public:
    int  k1, k2, k3, k4, k5, k6, k7, k8;
    int k9, k10, k11;
    double Y1, Y4, Y5, Y6, Y7, Y8, Y9;
    double Y2, Y3, Y10;
    double Y1_his;
    double Y4_his;
    double Y5_his;
    double Y6_his;
    double Y9_his;
    double Y8_his;
    double Y7_his;

    double T1;
    double PI_OUT;

};

class rate_limiter
{
public:
    rate_limiter()
    {
        output_his = 0.0;
        output = 0.0;
    }
    void ExecuteOperation(double input, double dt, double ULIM, double LLIM)
    {
        rate = (input - output_his) / dt;
        if (rate > 0)
        {
            if (rate > ULIM)
            {
                output = output_his + ULIM * dt;
            }
            else
            {
                output = input;
            }
        }
        else
        {
            if (fabs(rate) >= LLIM)
            {
                output = output_his - LLIM * dt;
            }
            if (fabs(rate) < LLIM)
            {
                output = input;
            }
        }
        output_his = output;
    }
public:
    double output_his, rate;
    double output;
};

class rec_polar
{
public:
    rec_polar()
    {
        output_M = 0.0;
        output_P = 0.0;
    }
    void ExecuteOperation(double input_X, double input_Y)
    {
        output_M = sqrt(pow(input_X, 2) + pow(input_Y, 2));
        output_P = atan2(input_Y, input_X);
    }
public:
    double output_M, output_P;
};

class limiter
{
public:
    limiter()
    {
        output = 0.0;
    }
    void ExecuteOperation(double input, double MAX, double MIN)
    {
        if (input > MAX)
        {
            output = MAX;
        }
        else if (input < MIN)
        {
            output = MIN;
        }
        else
        {
            output = input;
        }
    }
public:
    double output;
};

class polar_rec
{
public:
    polar_rec()
    {
        output_X = 0.0;
        output_Y = 0.0;
    }
    void ExecuteOperation(double input_M, double input_P)
    {
        output_X = input_M * cos(input_P);
        output_Y = input_M * sin(input_P);
    }
public:
    double output_X, output_Y;
};

class DQ0_ABC
{
public:
    DQ0_ABC()
    {
        abc0 = 0.0;
        abc1 = 0.0;
        abc2 = 0.0;
    }
    void ExecuteOperation(double dq0, double dq1, double dq2, double theta)
    {
        abc0 = cos(theta) * dq1 - sin(theta) * dq2 + dq0;
        abc1 = cos(theta - 2.0 * acos(-1.0) / 3.0) * dq1 - sin(theta - 2.0 * acos(-1.0) / 3) * dq2 + dq0;
        abc2 = cos(theta + 2.0 * acos(-1.0) / 3.0) * dq1 - sin(theta + 2.0 * acos(-1.0) / 3.0) * dq2 + dq0;
        output0 = abc0;
        output1 = abc1;
        output2 = abc2;
    }

public:
    double abc0;
    double abc1;
    double abc2;
    double output0;
    double output1;
    double output2;
};

class MPPT
{
public:
    MPPT(double Samp, double DELT, double Strtup)
    {
        output_SampNum = int(Samp * 0.001 / DELT) + 1;
        SampleCount = output_SampNum;
        output_StrtupNum = int(Strtup / Samp * 1000) + 1;
        StrtupCount = output_StrtupNum;
        StartBlk = 0;
        Reset_old = 0;
        Vold = 0;
        Iold = 0;
        //Pold = 0;
        Vmppt_old = 0;


        //Store_SampleCount = 0;
        //Store_StrtupCount = 0;
        //Store_StartBlk = 0;


    }
    void ExecuteOperation(int Reset, double Vinput, double Iinput, double Sd, double Vstep)
    {
        //SampleCount = Store_SampleCount;
        //StrtupCount = Store_StrtupCount;
        //StartBlk = Store_StartBlk;
        //Reset_old = Store_Reset_old;
        //Vold = Store_Vold;
        //Iold = Store_Iold;
        //Vmppt_old = Store_Vmppt_old;

        if ((Reset - Reset_old) == 1)
        {
            output_State = 0;
        }
        Reset_old = Reset;

        if (SampleCount > 0)
        {
            SampleCount = SampleCount - 1;
        }

        if (SampleCount == 0)
        {
            if (output_State == 0)
            {
                if (StartBlk == 0)
                {
                    output_Vopen = Vinput;
                    output_VstartStep = output_Vopen / output_StrtupNum;
                    StartBlk = 1;
                    output_Vmppt = 0;
                }

                output_Vmppt = output_Vmppt + output_VstartStep;
                StrtupCount = StrtupCount - 1;

                if (StrtupCount == 0)
                {
                    output_State = 1;
                    StrtupCount = output_StrtupNum;
                    StartBlk = 0;

                }
            }

            if (output_State == 1)
            {
                Vmppt_old = output_Vmppt;
                dI = Iinput - Iold;
                dV = Vinput - Vold;
                dP = Iinput * Vinput - Iold * Vold;

                Iold = Iinput;
                Vold = Vinput;

                if (dV != 0)
                {
                    Cond = (dI / dV) * Vinput + Iinput;
                    if (Cond > Sd)
                        output_Vmppt = output_Vmppt + Vstep * 0.001;

                    if (Cond < (-1.0 * Sd))
                        output_Vmppt = output_Vmppt - Vstep * 0.001;
                }
                else
                {
                    if (dI > 0.0)
                    {
                        output_Vmppt = output_Vmppt + Vstep * 0.001;
                    }
                    if (dI < 0.0)
                    {
                        output_Vmppt = output_Vmppt - Vstep * 0.001;
                    }
                }


            }
            if ((output_Vmppt > output_Vopen) || (output_Vmppt < 0.0))
            {
                output_Vmppt = Vmppt_old;
            }

            SampleCount = output_SampNum;
        }
        output_OUT = Cond;
        //Store_SampleCount = SampleCount;
        //Store_StrtupCount = StrtupCount;
        //Store_StartBlk = StartBlk;
        //Store_Reset_old = Reset_old;
        //Store_Vold = Vold;
        //Store_Iold = Iold;
        //Store_Vmppt_old = Vmppt_old;

        Vmppt_old = output_Vmppt;
        Store_Vold = Vold;
        Store_Iold = Iold;
        Store_StartBlk = StartBlk;
        Store_SampleCount = SampleCount;
        Reset = Reset_old;
        Store_StrtupCount = StrtupCount;


    }
public:
    int SampleCount;
    int StrtupCount;
    int Store_SampleCount;
    int Store_StrtupCount;
    //int Store_StrtupCount;
    int StartBlk;
    int Reset_old;
    int Store_StartBlk;
    int Store_Reset_old;

    double Vold, Iold;
    double Vmppt_old;
    int Store_Vold, Store_Iold, Store_Vmppt_old;

    double dI, dV, dP;
    double Cond;

    int output_State;
    double output_Vmppt;
    double output_VstartStep;
    double output_Vopen;
    int output_SampNum;
    double output_OUT;

    int output_StrtupNum;




};

class LPF2
{
public:
    LPF2()
    {
        output = 0.0;
        output_his = 2.19759;
        input_his = 0.0;
    }

    void ExecuteOperation(double Kf, double G, double input)
    {
        output = 1.0 / (1 + Kf) * G * (input_his + input) + (Kf - 1) / (Kf + 1) * output_his;
        output_his = output;
        input_his = input;
    }

public:
    // 输出量
    double output;
    double output_his, input_his;
};

class Interpolated_Firing_Pulses
{
public:
    Interpolated_Firing_Pulses()
    {
        output1 = 0.0;
        output2 = 0;
        fx_his = 0.0;
        fx = 0.0;

    }


    void ExecuteOperation(int Dblok, double H_OFF, double L_OFF, double H_ON, double L_ON, double delt)
    {

        if (Dblok == 0)
        {
            output1 = 0;
            output2 = 0.0;
        }
        else
        {
            if (H_OFF >= L_OFF)
            {
                output1 = 0;
            }
            else
            {
                output1 = 1;
            }
            if (H_ON >= L_ON)
            {
                output1 = 1;
            }
            else
            {
                output1 = 0;
            }
            fx = L_OFF - H_OFF;

            //output2 = abs(fx) * delt /(abs(fx) + abs(fx_his));
            if ((fx_his * fx) <= 0)
            {
                output2 = fabs(fx) * delt / (fabs(fx) + fabs(fx_his));
            }
            else
                output2 = 0.0;


            fx_his = fx;
        }

    }
public:
    int output1;
    double output2;
    double fx;
    double fx_his;
};

class Sawtooth_Generator
{
public:
    Sawtooth_Generator()
    {
        output = 0.0;
    }
    void ExecuteOperation(double input_freq, double max_pwm, double min_pwm, double DELT)
    {
        NumCount = 1.0 / (input_freq * DELT);
        slope1 = (max_pwm - (max_pwm + min_pwm) / 2.0) * 4.0;
        //slope2 = 2.0 * (max_pwm - min_pwm);
        slope2 = -slope1;
        Count_i = Count_i + 1.0 / NumCount;


        while (Count_i < 0)
        {
            Count_i = Count_i + 1;
        }
        while (Count_i > 1)
        {
            Count_i = Count_i - 1;
        }

        if (Count_i >= 0 && Count_i < 0.25)
        {
            //intercept = 0.0;
            intercept = (max_pwm + min_pwm) / 2.0;
            slope = slope1;

        }
        else if (Count_i >= 0.25 && Count_i < 0.75)
        {
            slope = slope2;
            //intercept = (max_pwm - min_pwm);
            intercept = min_pwm + 3.0 / 2.0 * (max_pwm - min_pwm);
        }
        else
        {
            slope = slope1;
            intercept = min_pwm - (3.0 / 2.0) * (max_pwm - min_pwm);
        }

        output = slope * Count_i + intercept;
    }
public:
    double NumCount, slope, slope1, slope2, intercept;
    double Count_i;
    double output;


};

class LPFF
{
public:
	LPFF()
	{
		output = 0.0;
		output_his = 0.0;
		input_his = 0.0;
	}

	void ExecuteOperation(double Kf, double G, double input)
	{
		output = 1.0 / (1 + Kf) * G * (input_his + input) + (Kf - 1) / (Kf + 1) * output_his;
		output_his = output;
		input_his = input;
	}

public:
	// �����
	double output;
	double output_his, input_his;
};
